#ifndef _PILINK_H_
#define _PILINK_H_



/**************************************************************************************************
 *  
 *                            Structure of Pilink message:
 * 
 * ------------------------------------------------------------------------------------------------
 * Segment:  |  Header   | Sender id |  Type     |  Length   |   Data    |  Checksum  |   Tail    |
 * ------------------------------------------------------------------------------------------------
 * Data Type:|  uint16_t | uint8_t   |  uint16_t |  uint16_t  |  uint8_t  |  uint8_t   |  uint16_t |
 * ------------------------------------------------------------------------------------------------
 * Length:   |  2 Bytes  |  1 Byte   |  2 Byte   |  2 Byte    |  N Bytes  |   1 Byte   |  uint16_t |
 * ------------------------------------------------------------------------------------------------
 * 
 *  1. checksum is sum of Data
 *  2. N is indicated by Length 
 * 
 * ***********************************************************************************************/



#define PILINK_HEAD1   0xFA
#define PILINK_HEAD2   0xAA

#define PILINK_END1    0xFE
#define PILINK_END2    0xFD

#define PILINK_HANDLER_BUFFER_LENGTH  512


#ifdef __cplusplus
extern "C" {
#endif

enum pilink_status_e{
    GSTATUS_IDLE = 0,
    GSTATUS_HEAD,
    GSTATUS_SENDER,
    GSTATUS_PORT1,
    GSTATUS_PORT2,
    GSTATUS_LEN1,
    GSTATUS_LEN2,
    GSTATUS_DATA,
    GSTATUS_CHECK,
    GSTATUS_END1,
    GSTATUS_END2,
};

struct pilink_handler_s{
    enum pilink_status_e status;
    
    unsigned char   sender;
    unsigned short  port;
    unsigned short  length;
    unsigned char   checksum;
    unsigned short  wptr;
    unsigned char   buf[PILINK_HANDLER_BUFFER_LENGTH];
    
};


#define PILINK_DATA_TYPE_FLOAT             0x0001
#define PILINK_DATA_TYPE_INT               0x0002
#define PILINK_DATA_TYPE_STRING            0x0003
#define PILINK_DATA_TYPE_MESSAGE           0x0004

#define PILINK_DATA_TYPE_HEARTBEAT         0x0101
#define PILINK_DATA_TYPE_IMU               0x0102
#define PILINK_DATA_TYPE_MAG               0x0103
#define PILINK_DATA_TYPE_BARO              0x0104
#define PILINK_DATA_TYPE_GPS               0x0105
#define PILINK_DATA_TYPE_MANUAL            0x0106

#define PILINK_DATA_TYPE_ATT               0x0203
#define PILINK_DATA_TYPE_VIO               0x0204
#define PILINK_DATA_TYPE_MOTION            0x0205
#define PILINK_DATA_TYPE_SETPOINT          0x0206
#define PILINK_DATA_TYPE_LOCAL_POS         0x0207

#define PILINK_DATA_TYPE_GLANN_INFO        0x0701
#define PILINK_DATA_TYPE_GLANN_WEIGHT      0x0702

#pragma pack(1)

struct pilink_struct_heartbeat_s{
    unsigned long long timestamp;
};

struct pilink_struct_imu_s{
    unsigned long long timestamp;
    float acc[3];
    float gyro[3];
};

struct pilink_struct_manual_s{
    unsigned long long timestamp;
    float channel[16];
};

struct pilink_struct_vio_s{
    unsigned long long timestamp;
    float vel[3];
    float pos[3];
    float q[4];
	float quality[3];
};

struct pilink_struct_motion_info_s{
    unsigned long long timestamp;
    float pos[3];
    float vel[3];
    float acc[3];
    float q[4];
};

struct pilink_struct_pos_setpoint_s{
    unsigned long long timestamp;
    float pos[3];
	float vel[3];
	float acc[3];
};

struct pilink_struct_att_s{
    unsigned long long timestamp;
    float q[4];
    float rate[3];
};

struct pilink_struct_local_pos_s{
    unsigned long long timestamp;
    float pos[3];
    float vel[3];
    float acc[3];
};

struct pilink_struct_setpoint_s{
    unsigned long long timestamp;
    float pos[3];
    float vel[3];
    float acc[3];
    float qatt[4];
    float rate[3];
    float force[3];
    float torque[3];
};

struct pilink_struct_actuator_s{
    unsigned long long timestamp;
    float channel[8];
};

struct pilink_struct_mag_s{
    unsigned long long timestamp;
    float data[3];
};

struct pilink_struct_baro_s{
    unsigned long long timestamp;
    float pressure;
    float altitude;
    float temperature;
};

struct pilink_struct_message_s{
    unsigned long long timestamp;
    unsigned char len;
    char msg[128];
};

struct pilink_struct_glann_info_s{
    unsigned long long timestamp;
    unsigned int       layer_num;
    float              learning_rate;
    unsigned short     neure_num[PILINK_HANDLER_BUFFER_LENGTH / 8 ];
    unsigned char      neure_type[PILINK_HANDLER_BUFFER_LENGTH / 8];
};

struct pilink_struct_glann_weight_s{
    unsigned long long timestamp;
    unsigned int       layer;
    unsigned short     neure_forward;
    unsigned short     neure_current;
    float              weight;
};

#pragma pack()

void            pilink_handler_init(struct pilink_handler_s *handler);
int             pilink_handler_parse(struct pilink_handler_s *handler,unsigned char ch);
unsigned char   pilink_handler_calc_checksum(struct pilink_handler_s *handler);
unsigned char*  pilink_handler_get_data(struct pilink_handler_s *handler);
int             pilink_msg_pack(unsigned char *buffer,unsigned char sender,unsigned short port, unsigned char *dat, unsigned short len);

#ifdef __cplusplus
}
#endif
#endif
